/**
 * @file DF.h
 * @author rj.wang (rj.wang@aotbot.com)
 * @brief 障碍物距离（栅格距离平方值）地图
 * @version 0.1
 * @date 2024-11-04
 * @copyright Copyright (c) 2024
 */

#ifndef __COMMON_PLANNING_DF_H_
#define __COMMON_PLANNING_DF_H_
#include <iostream>
#include <limits>
#include <stdint.h>
#include <cmath>
#include <vector>
#include <set>
#include <deque>

#include "GridMap.h"

namespace common {
namespace planning {

struct FPoint {
  FPoint(float a = 0, float b = 0): x(a), y(b) {}
  FPoint(const FPoint& fp): x(fp.x), y(fp.y) {}
  FPoint& operator=(const FPoint& fp) {
    x = fp.x;
    y = fp.y;
    return *this;
  }
  float x;
  float y;
};
using FPointVec = std::vector<FPoint>;

enum LnOrCol{
  LN = 0,
  COL = 1
};

struct CostGrid {
  CostGrid(int64_t a = 0, int64_t b = 0, float c = 0.0): x(a), y(b), cost(c) {}
  CostGrid(const CostGrid& cg): x(cg.x), y(cg.y), cost(cg.cost) {}
  bool operator < (const CostGrid& cg) const { return (x < cg.x) || (x == cg.x && y < cg.y); }
  int64_t x;
  int64_t y;
  float cost;
};
using CostGridVec = std::vector<CostGrid>;
using CostGridQueue = std::deque<CostGrid>;
using CostGridSet = std::set<CostGrid>;

class DistanceMap : public GridMap<float> {
private:
  //grid's squared distance <= thresh_obstacle, means robot will bump here! 
  float thresh_obstacle_; 
  //grid's squared distance >= thresh_free, means it's safe enough for robot to walk! 
  float thresh_free_; 
  //for calculate pass difficulty
  float factor_difficulty_;

private:
  float PassDifficulty(
      const int64_t neighbour_x, 
      const int64_t neighbour_y) const;

public:
  inline void SetGrid(
      const int64_t x, 
      const int64_t y, 
      const float distance) { 
    data_.at(x + y * width_) = distance; 
  }

  inline void SetGridOccupied(const int64_t x, const int64_t y) { 
    data_.at(x + y * width_) = 0.0; 
  }

  inline void SetGridFree(const int64_t x, const int64_t y) { 
    data_.at(x + y * width_) = std::numeric_limits<float>::max(); 
  }

  inline float GetGrid(const int64_t x, const int64_t y) const { 
    return data_.at(x + y * width_); 
  }

  //below should be used with graph search algorithm
  CostGridVec GetNeighbours(int64_t x, int64_t y) const;

  CostGridVec EightNeighbours(int64_t x, int64_t y) const;

  float Heuristic(
      const int64_t cur_x, 
      const int64_t cur_y, 
      const int64_t goal_x, 
      const int64_t goal_y) const;

  bool Occupied(const int64_t x, const int64_t y) const;

  inline void SetObstacleThresh(const float th) { 
    thresh_obstacle_ = th; 
  }

  inline void SetFreeThresh(const float th) { 
    thresh_free_ = th; 
  }

  inline void CalDifficultyFactor() { 
    factor_difficulty_ = 
      fabs(thresh_free_ - thresh_obstacle_) < 1e-3 ? 
        0 : 
        1.5 / (thresh_free_ - thresh_obstacle_);  //1.5 - weight of clearance
  }

  inline void SetObstacleFreeThresh(
      const float th_obs, 
      const float th_f) {
    SetObstacleThresh(th_obs);
    SetFreeThresh(th_f);
    CalDifficultyFactor();
  }

  inline float ThreshFree() const { 
    return thresh_free_; 
  }

  inline float ThreshObstacle() const { 
    return thresh_obstacle_; 
  }

  void PrintMap() const;

  void PrintMap(const float threshold) const;
};

//for graph search
int64_t GetFreeCount(
    const DistanceMap& map, 
    const int64_t xmin, 
    const int64_t ymin, 
    const int64_t xmax, 
    const int64_t ymax);

CostGridSet GetConnected(
    const DistanceMap& map, 
    const int64_t x, 
    const int64_t y, 
    const int64_t count_thresh = 100);

}//end of namespace planning
}//end of namespace common

#endif // __COMMON_PLANNING_DF_H_